//
// Created by yongj on 2021/7/30.
//

#ifndef CAR_BALANCE_P36_H
#define CAR_BALANCE_P36_H

#include "../mcp2515/mcp2515.h"
#include "types.h"
#include "stdlib.h"

/** \brief 初始化P36电机
 * */
void p36_init();

/** \brief 获得电机当前运行状态
 * \param id 要获取的点掉ID
 * \param state_data 返回的数据
 * */
void p36_get_motor_state();

/** \brief 获取电机机械角度
 * \param id 电机ID
 * \return 电机机械角度 0-8191————0-360
 * */
float p36_get_motor_angle(unsigned long id);

/** \brief 获取电机速度
 * \param id 电机ID
 * \return 电机速度 RPM
 * */
int16_t p36_get_motor_speed(unsigned long id);

/** \brief 获取电机转矩
 * \param id 电机ID
 * \return 电机转矩
 * */
uint16_t p36_get_motor_torque(unsigned long id);

/** \brief 发送电机电流设置数据
 * */
void p36_put_motor_current();

/** \brief 设置电机电流大小
 * \param id 电调ID
 * \param current 电流 -10000~0~10000
 * */
void p36_set_motor_current(unsigned long id, int16_t current);

/** \brief 读取电机速度
 * */
bool getSpeed();

/** \brief 发送电机状态
 * */
bool sendDATA();

/** \brief 发送电调控制信息
 * 电调在2S内未收到信息将停止转动
 * */
bool keep_control();

/** \brief 初始化PID数值
 * */
void PID_init(pid *setPID);

/** \brief PID控制调用
 * */
void PID_get(pid *pid_set);

/** \brief 设置PID算法参数
 * \param Kp P值
 * \param Ki I值
 * \param Kd D值
 * */
void PID_set(float Kp, float Ki, float Kd);
void PID_set_Kp(float Kp);
void PID_set_Ki(float Ki);
void PID_set_Kd(float Kd);

/** \brief 设置电机转速大小
 * \param id 电调ID
 * \param RPM 设置的转速
 * */
void p36_set_motor_RPM(unsigned long id, int16_t RPM);
int p36_get_motor_RPM(unsigned long id);

void p36_state_set_STOP();

/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回  值：直立控制PWM
**************************************************************************/
int16_t balance(float Angle,float Gyro);

/**************************************************************************
函数功能：速度PI控制
入口参数：电机编码器的值
返回  值：速度控制PWM
作    者：大鱼电子
**************************************************************************/
int velocity(int Encoder_Left,int Encoder_Right,float Angle);

#endif //CAR_BALANCE_P36_H
